#include "bezierCurveEvaluator.h"
#include <cassert>
#include <cmath>

#define STEPS 25.0



static Point bezierQ(const std::vector<Point> g, const double t)
{
	double vx[4], vy[4];
	
	vx[0] = -1 * g[0].x +  3 * g[1].x + -3 * g[2].x + g[3].x;
	vx[1] =  3 * g[0].x + -6 * g[1].x +  3 * g[2].x;
	vx[2] = -3 * g[0].x +  3 * g[1].x	;
	vx[3] = 		 g[0].x;
	
	vy[0] = -1 * g[0].y +  3 * g[1].y + -3 * g[2].y + g[3].y;
	vy[1] =  3 * g[0].y + -6 * g[1].y +  3 * g[2].y;
	vy[2] = -3 * g[0].y +  3 * g[1].y	;
	vy[3] = 		 g[0].y;
	
	Point ret = Point(vx[3] + t * (vx[2] + t * (vx[1] + t * vx[0])),
										vy[3] + t * (vy[2] + t * (vy[1] + t * vy[0]))); 
	

//	printf("bezierQ: g:\n(%f, %f)\n(%f, %f)\n(%f, %f)\n(%f, %f)\nt = %f = (%f, %f)\n", 
//									g[0].x, g[0].y, g[1].x, g[1].y, g[2].x, g[2].y, g[3].x, g[3].y, t, ret.x, ret.y);

	return ret;	
}

void BezierCurveEvaluator::evaluateCurve(const std::vector<Point>& _ptvCtrlPts, 
					 std::vector<Point>& ptvEvaluatedCurvePts, 
					 const float& fAniLength, 
					 const bool& bWrap) const
{
	std::vector<Point> ptvCtrlPts;	//	we'll make this into the magically working list of points to evaluate
	int n = _ptvCtrlPts.size();	//	number of real control points
	float dWrap = _ptvCtrlPts[0].x + (fAniLength - _ptvCtrlPts.back().x);	//	distance from last point to first point over wrap
	
	//	go go gadget hack
	
	if(!(bWrap && (n % 3 == 0))) {
		ptvCtrlPts.push_back(Point(-4.0, _ptvCtrlPts[0].y));
		ptvCtrlPts.push_back(Point(-4.0, _ptvCtrlPts[0].y));	}
	
	if(bWrap && (n % 3 != 0)) {
		ptvCtrlPts.push_back(Point(_ptvCtrlPts.front().x - dWrap, _ptvCtrlPts.back().y));
		ptvCtrlPts.push_back(Point(_ptvCtrlPts.front().x - dWrap, _ptvCtrlPts.back().y));
		ptvCtrlPts.push_back(_ptvCtrlPts.front());
	}
	
	if(!bWrap || (n % 3 != 0)) ptvCtrlPts.push_back(_ptvCtrlPts.front());
	
	ptvCtrlPts.insert(ptvCtrlPts.end(), _ptvCtrlPts.begin(), _ptvCtrlPts.end() );	//	insert the real points
	
	if(bWrap && (n % 3 == 0)) {
		ptvCtrlPts.push_back(Point(_ptvCtrlPts.back().x + dWrap, _ptvCtrlPts.front().y));
		ptvCtrlPts.push_back(Point(_ptvCtrlPts.back().x + dWrap, _ptvCtrlPts.front().y));
		
	}

	ptvCtrlPts.push_back(Point(_ptvCtrlPts[n-1].x, _ptvCtrlPts[n-1].y));
	ptvCtrlPts.push_back(Point(_ptvCtrlPts[n-1].x, _ptvCtrlPts[n-1].y));
	ptvCtrlPts.push_back(Point(fAniLength+4, _ptvCtrlPts[n-1].y));
	ptvCtrlPts.push_back(Point(fAniLength+4, _ptvCtrlPts[n-1].y));
	
	//	end hack
	
	
	ptvEvaluatedCurvePts.clear();
	std::vector<Point> g = std::vector<Point>(4);
	if(bWrap && (n % 3 == 0)) {
		//	fake the curve off the beginning
		g[0] = Point(0 - (fAniLength - _ptvCtrlPts[n-3].x), _ptvCtrlPts[n-3].y);
		g[1] = Point(0 - (fAniLength - _ptvCtrlPts[n-2].x), _ptvCtrlPts[n-2].y);
		g[2] = Point(0 - (fAniLength - _ptvCtrlPts[n-1].x), _ptvCtrlPts[n-1].y);
		g[3] = _ptvCtrlPts.front();
		for(int i = 0; i <= STEPS; i++) ptvEvaluatedCurvePts.push_back(bezierQ(g, (float)i/STEPS));
	}
	
	//	do the interpolation
	int i = 0;
	while(i < ptvCtrlPts.size() - 7) {
		for(int j = 0; j < 4; j++) g[j] = ptvCtrlPts[i+j];
		for(int j = 0; j <= STEPS; j++)	ptvEvaluatedCurvePts.push_back(bezierQ(g, (float)j/STEPS));
		i += 3;	
	}
	
	//	handle linear case when we aren't wrapping a curve around the end
	if((n % 3 == 0) && !bWrap) {
		ptvEvaluatedCurvePts.push_back(_ptvCtrlPts[_ptvCtrlPts.size() - 2]);
		ptvEvaluatedCurvePts.push_back(_ptvCtrlPts.back());
		ptvEvaluatedCurvePts.push_back(Point(fAniLength + 4.0, ptvEvaluatedCurvePts.back().y));
	}
	
	switch(n % 3) {
		case 2:
			ptvEvaluatedCurvePts.push_back(_ptvCtrlPts.back());
		case 1:
			if(!bWrap) {
				ptvEvaluatedCurvePts.push_back(Point(fAniLength + 4.0, ptvEvaluatedCurvePts.back().y));
			} else {
				ptvEvaluatedCurvePts.push_back(Point(_ptvCtrlPts.back().x + dWrap, _ptvCtrlPts.front().y));
			}
	}

	//printf("n = %d, size = %d, n%%3 = %d\n", n, ptvCtrlPts.size(), n%3);
}
